#include "safety_recheck_itf.h"


int check_jpos_limit(double current_jpos_fb, double max_jpos_limit, double min_jpos_limit)
{
    
    if((current_jpos_fb > max_jpos_limit) || (current_jpos_fb < min_jpos_limit))
    {
        return 1;
    }
    return 0;
}

/*
judge jvel, return 1: jvel outof limit
*/
int check_jvel_limit(double current_jvel, double max_jvel_limit)
{    
    if(fabs(current_jvel) > max_jvel_limit)
    {
        return 1;
    }
    return 0;
}

int check_linear_vel_limit(double linear_vel, double linear_vel_limit)
{
    if(fabs(linear_vel) > linear_vel_limit)
    {
        return 1;
    }
    return 0;
}

//NOTE: curPose 与示教平面的点处于同一坐标系下
int check_safe_plane(SafePlane plane, ZucPose *curPose, double *dist, int elbow_limit_flag, double r_elbow)
{
    int ret = 0;
    double d;
    double safeD = 0.0;
    switch (elbow_limit_flag)
    {
    case 0:
        safeD = plane.safeD;
        break;
    case 1:
        safeD = r_elbow + plane.safeD;
        break;
    default:
        break;
    }

    *dist = plane.norm.point.x * curPose->tran.x + plane.norm.point.y * curPose->tran.y + plane.norm.point.z * curPose->tran.z + plane.d;
    d = plane.norm.point.x * plane.safePoint.point.x + plane.norm.point.y * plane.safePoint.point.y + plane.norm.point.z * plane.safePoint.point.z + plane.d;
    if (d > 0.0)
    {
        *dist = *dist - safeD;
    }
    else
    {
        *dist = *dist + safeD;
    }

    *dist = (d * (*dist)) / fabs(d);
    ret = (*dist) > 0 ? 0 : 1;  // on dangeous side
    return ret;
}

int check_tool_ori(ZucPose *cart_pos, ZucPose *cur_tool_offsets, ToolOriCons *tool_ori_cons, double* incl_angle)
{
    int ret = 0;
    PmRpy cur_ori_limited_rpy;
    PmRotationMatrix cur_ori_limited_mat;
    cur_ori_limited_rpy.y = cart_pos->a * PM_PI / 180.0;
    cur_ori_limited_rpy.p = cart_pos->b * PM_PI / 180.0;
    cur_ori_limited_rpy.r = cart_pos->c * PM_PI / 180.0;
    pmRpyMatConvert(&cur_ori_limited_rpy, &cur_ori_limited_mat);

    if(tool_ori_cons->conical_tip_choice == 0)
    {
        //move tool_offset,变换得到末端法兰{end}相对于{world}的oritation
        PmRpy tool_rpy = {0};
        tool_rpy.y = cur_tool_offsets->a * PM_PI / 180.0;
        tool_rpy.p = cur_tool_offsets->b * PM_PI / 180.0;
        tool_rpy.r = cur_tool_offsets->c * PM_PI / 180.0;
        PmRotationMatrix tool_mat = {0}, tool_mat_inv = {0};
        pmRpyMatConvert(&tool_rpy, &tool_mat);
        pmMatInv(&tool_mat, &tool_mat_inv);
        pmMatMatMult(&cur_ori_limited_mat, &tool_mat_inv, &cur_ori_limited_mat);
    } 
    // transform: z_to_to_be_constrained related to {tool} or {end} 
    double x_tilt = tool_ori_cons->tool_properties_tilt * PM_PI / 180.0;
    double z_pan = tool_ori_cons->tool_properties_pan * PM_PI / 180.0;
    double s_x_tilt = sin(x_tilt);
    double c_x_tilt = cos(x_tilt);
    double s_z_pan = sin(z_pan);
    double c_z_pan = cos(z_pan);
    PmRotationMatrix R_x, R_z;
    R_x.x.x = 1;     R_x.y.x = 0;     R_x.z.x = 0;
    R_x.x.y = 0;     R_x.y.y = c_x_tilt;     R_x.z.y = -s_x_tilt;
    R_x.x.z = 0;     R_x.y.z = s_x_tilt;     R_x.z.z = c_x_tilt;

    R_z.x.x = c_z_pan;     R_z.y.x = -s_z_pan;     R_z.z.x = 0;
    R_z.x.y = s_z_pan;     R_z.y.y = c_z_pan;     R_z.z.y = 0;
    R_z.x.z = 0;     R_z.y.z = 0;     R_z.z.z = 1;
    PmRotationMatrix new_ori_to_old_ori;
    pmMatMatMult(&R_z, &R_x, &new_ori_to_old_ori);
    pmMatMatMult(&cur_ori_limited_mat, &new_ori_to_old_ori, &cur_ori_limited_mat);//TODO: 更换为相对于基座坐标系

    PmRpy rpy_conical;
    rpy_conical.y = tool_ori_cons->conical_cenrpy[0] * PM_PI / 180.0;
    rpy_conical.p = tool_ori_cons->conical_cenrpy[1] * PM_PI / 180.0;
    rpy_conical.r = tool_ori_cons->conical_cenrpy[2] * PM_PI / 180.0; 
    PmRotationMatrix mat_conical;
    pmRpyMatConvert(&rpy_conical, &mat_conical);

    double mag_of_mat_conical_z = pmSqrt(pmSq(mat_conical.z.x) + pmSq(mat_conical.z.y) + pmSq(mat_conical.z.z));
    //calculate cur_pos_ori angle
    double cur_mag_mult = pmSqrt(pmSq(cur_ori_limited_mat.z.x) + pmSq(cur_ori_limited_mat.z.y) + pmSq(cur_ori_limited_mat.z.z)) * mag_of_mat_conical_z;
    double cur_dot_vec = (cur_ori_limited_mat.z.x * mat_conical.z.x + cur_ori_limited_mat.z.y * mat_conical.z.y + cur_ori_limited_mat.z.z * mat_conical.z.z) / cur_mag_mult;
    if (cur_dot_vec > 1.0)
        cur_dot_vec = 1.0;
    else if (cur_dot_vec < -1.0)
        cur_dot_vec = -1.0;
    *incl_angle = fabs(acos(cur_dot_vec));

    if(*incl_angle > tool_ori_cons->deviation * PM_PI / 180.0)
    {
        ret = 1;
    }
    return ret;
}
       
                            
    